/**
 * @copyright 2021 Xoan Iago Suarez Canosa. All rights reserved.
 * Constact: iago.suarez@thegraffter.com
 * Software developed in the PhD: Low-level vision for resource-limited devices
 */
#include <opencv2/opencv.hpp>
#include "BAD.h"
#include "HashSIFT.h"
#include <fstream>

using namespace upm;

inline cv::Mat drawGoodMatches(const cv::Mat &query,
                               const cv::Mat &pattern,
                               const std::vector<cv::KeyPoint> &queryKp,
                               const std::vector<cv::KeyPoint> &trainKp,
                               const std::vector<cv::DMatch> &matches) {
  cv::Mat outImg;

  if (query.empty() || pattern.empty()) {
    std::cerr << "ERROR in drawGoodMatches: The input image is empty." << std::endl;
    return outImg;
  }

  if (queryKp.empty() || trainKp.empty() || matches.empty()) {
    return outImg;
  }

  cv::drawMatches(query, queryKp, pattern, trainKp, matches, outImg,
                  CV_RGB(0, 255, 0), CV_RGB(255, 0, 0),
                  std::vector<char>(), cv::DrawMatchesFlags::DEFAULT);

  cv::putText(outImg,
              std::string("Inliers: ") + std::to_string(matches.size()),
              cv::Point(20, query.rows - 20),
              cv::FONT_HERSHEY_SIMPLEX,
              1,
              CV_RGB(0, 255, 0),
              2);

  return outImg;
}

std::vector<cv::KeyPoint> readKeypointsFromFile(const std::string& filename) {
    std::ifstream infile(filename);
    if (!infile.is_open()) {
        std::cerr << "Error opening file!" << std::endl;
        return std::vector<cv::KeyPoint>();
    }

    std::vector<cv::KeyPoint> keypoints;
    float x, y, score;
    while (infile >> x >> y >> score) {
        // std::cout<<x<<","<<y<<std::endl;
        cv::KeyPoint kp(x, y, 1.0); // 默认大小为 1.0
        kp.response = score; // 设置关键点的响应值为 score
        keypoints.push_back(kp);
    }

    infile.close();
    return keypoints;
}


void exportMatToTxt(const cv::Mat& mat, const std::string& filename) {
    std::ofstream outfile(filename);
    if (!outfile.is_open()) {
        std::cerr << "Error opening file!" << std::endl;
        return;
    }

    // 遍历矩阵的每个元素，并将其写入文件
    for (int i = 0; i < mat.rows; ++i) {
        for (int j = 0; j < mat.cols; ++j) {
            outfile << mat.at<float>(i, j) << " "; // 假设矩阵元素类型为 float
        }
        outfile << "\n"; // 换行
    }

    outfile.close();
}

int main(int argc, char *argv[]) {
  /////// 1 ==> 256 bit
  ///////  ./hash_sift_func imgs/1403638543027829504.png Test.keypoints A.txt 1

  //Get input image path from arguments if provided
  std::string imageInputPath;
  std::string inputPath_kpts;
  std::string outputPath_desc;
  int desclen_type;

  if (argc == 5) {
      imageInputPath = argv[1];
      inputPath_kpts= argv[2];
      outputPath_desc= argv[3];
      desclen_type=std::stoi(argv[4]);
  } else {
      imageInputPath = "imgs/1403638539877829376.png";
      inputPath_kpts="Test.keypoints";
      outputPath_desc="AA.desc";
      desclen_type=1;

  }

  cv::Ptr<cv::Feature2D> descriptor;
  if(desclen_type==1){
    descriptor = HashSIFT::create(1.0f, HashSIFT::SIZE_256_BITS);
  }else{
    descriptor = HashSIFT::create(1.0f, HashSIFT::SIZE_512_BITS);
  }
  cv::Mat img1 = cv::imread(imageInputPath, cv::IMREAD_GRAYSCALE);
  std::vector<cv::KeyPoint> kps1=readKeypointsFromFile(inputPath_kpts);




  cv::Mat descrs1;

  descriptor->compute(img1, kps1, descrs1);

  exportMatToTxt(descrs1,outputPath_desc);


  return 0;
}